// --------------------------------------------------------------------------------------------------------------------
// <copyright file="AxisAngle.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics
{
    using System;

    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Defines a basis for a transformation. It can be converted to a matrix or Quaternion.
    /// The axis component in this class should always be normalized.
    /// A strict check is performed during construction and an exception will be thrown if
    /// the given axis is not a unit vector.
    /// </summary>
    public struct AxisAngle
    {
        /// <summary>
        /// Trace context for this class
        /// </summary>
        private static TraceContext axisAngleContext = TraceContexts.FromString(TraceContexts.Manip, "AxisAngle");

        /// <summary>
        /// The axis vector
        /// </summary>
        private Vector3 axis;

        /// <summary>
        /// The angle of the axis
        /// </summary>
        private double angle;

        /// <summary>
        /// Initializes a new instance of the AxisAngle struct.
        /// </summary>
        /// <param name="axis">Upright axis.  Its length has to be 1.</param>
        /// <param name="angle">Rotation angle in radians</param>
        public AxisAngle(Vector3 axis, double angle)
        {
            if (Math.Abs(1 - axis.Length()) > MathConstants.ErrorEpsilon)
            {
                // TODO: enable the following exeption when the time is right
                ////throw new ArgumentException("Length of rotation axis is not 1. Axis must be a unit vector.  Use static method AxisAngle.CreateAxisAngleFromNonUnitAxis instead.");

                TraceOut.Warning(axisAngleContext, "Axis length is {0}, which is not 1. Please make sure it is a unit vector, or use static method AxisAngle.CreateAxisAngleFromRotationalAxisNonUnitVector instead.", axis.Length());
            }

            this.axis = new Vector3(axis);
            this.angle = angle;
        }

        /// <summary>
        /// Initializes a new instance of the AxisAngle struct.
        /// </summary>
        /// <param name="a">AxisAngle to copy</param>
        public AxisAngle(AxisAngle a)
        {
            this.angle = a.angle;
            this.axis = new Vector3(a.axis);
        }

        /// <summary>
        /// Gets or sets vertical (upright) axis
        /// </summary>
        public Vector3 Axis
        {
            get
            {
                return this.axis;
            }

            set
            {
                this.axis = value;
            }
        }

        /// <summary>
        /// Gets or sets rotation in radians
        /// </summary>
        public double Angle
        {
            get
            {
                return this.angle;
            }

            set
            {
                this.angle = value;
            }
        }

        /// <summary>
        /// Create a new instance of AxisAngle struct from a rotational axis represented by a non-unit vector.
        /// </summary>
        /// <param name="axis">A non-unit vector representing a rotational axis.</param>
        /// <param name="angle">Rotational angle in radians.</param>
        /// <returns>An AxisAngle struct.</returns>
        public static AxisAngle CreateAxisAngleFromRotationalAxisNonUnitVector(Vector3 axis, double angle)
        {
            axis = Vector3.Normalize(axis);
            return new AxisAngle(axis, angle);
        }

        /// <summary>
        /// Retrieves a string representation of the AxisAngle.
        /// </summary>
        /// <returns>String that represents the AxisAngle.</returns>
        public override string ToString()
        {
            return string.Format("X:{0} Y:{1} Z:{2} Angle:{3}", this.axis.X, this.axis.Y, this.axis.Z, this.angle);
        }
    }
}
